#include "area_navigator/area_navigator.hpp"
#include <cmath>
#include <tf/transform_datatypes.h>

AreaNavigator::AreaNavigator() 
    : has_active_goal_(false),
      is_in_adjustment_(false)
{
    // 加载参数


    
    loadParameters();
    
    // // 初始化ROS组件
    initializeROSComponents();
    // 发布可视化标记
      marker_timer_ = nh_.createTimer(
        ros::Duration(0.1),  // 1Hz
        [this](const ros::TimerEvent& event) {
            this->publishVisualizationMarkers();
        }
    );
    ROS_INFO("Marker timer created");
        ROS_INFO("area navigator success");
}

void AreaNavigator::loadParameters()
{
    ros::NodeHandle private_nh("~");
    bool success = true;
    // TargetArea target_area;
    // // 调试：打印所有可用参数
    // std::vector<std::string> param_names;
    // private_nh.getParamNames(param_names);
    // ROS_INFO("=== avaliable param list ===");
    // for (const auto& name : param_names) {
    //     ROS_INFO("param: %s", name.c_str());
    // }
    
    // 加载中心点坐标（数组格式 [x, y, z]）
    XmlRpc::XmlRpcValue center_array;
    if (private_nh.getParam("region/center", center_array)) {
        if (center_array.getType() == XmlRpc::XmlRpcValue::TypeArray && center_array.size() == 3) {
            target_area.center.x = static_cast<double>(center_array[0]);
            target_area.center.y = static_cast<double>(center_array[1]);
            target_area.center.z = static_cast<double>(center_array[2]);
            // if (private_nh.getParam("region/center", target_area.center)) {
    
         ROS_INFO("center: [%.2f, %.2f, %.2f]", 

                     target_area.center.x, target_area.center.y, target_area.center.z);
        } else {
            ROS_ERROR("error");
            success = false;
        }
    } else {
        ROS_ERROR("cannot get center");
        success = false;
    }
    
    // 加载半径
    if (!private_nh.getParam("region/radius", target_area.radius)) {
        ROS_ERROR("cannot get radius");
        success = false;
    } else {
        ROS_INFO("radius success: %.2f", target_area.radius);
    }
    
    // 加载偏航容差
    if (!private_nh.getParam("region/adjust_yaw_tolerance", target_area.adjust_yaw_tolerance)) {
        ROS_ERROR("cannot get tolerance");
        success = false;
    } else {
        ROS_INFO("tolerance success: %.2f", target_area.adjust_yaw_tolerance);
    }
    
    // 加载目标位姿位置（数组格式 [x, y, z]）
    XmlRpc::XmlRpcValue position_array;
    if (private_nh.getParam("region/target_pose/position", position_array)) {
        if (position_array.getType() == XmlRpc::XmlRpcValue::TypeArray && position_array.size() == 3) {
            target_area.target_pose.position.x = static_cast<double>(position_array[0]);
            target_area.target_pose.position.y = static_cast<double>(position_array[1]);
            target_area.target_pose.position.z = static_cast<double>(position_array[2]);
            ROS_INFO("position success: [%.2f, %.2f, %.2f]", 
                     target_area.target_pose.position.x, 
                     target_area.target_pose.position.y, 
                     target_area.target_pose.position.z);
        } else {
            ROS_ERROR("wrong position");
            success = false;
        }
    } else {
        ROS_ERROR("cennot get position");
        success = false;
    }
    
    // 加载目标位姿方向（数组格式 [x, y, z, w]）
    XmlRpc::XmlRpcValue orientation_array;
    if (private_nh.getParam("region/target_pose/orientation", orientation_array)) {
        if (orientation_array.getType() == XmlRpc::XmlRpcValue::TypeArray && orientation_array.size() == 4) {
            target_area.target_pose.orientation.x = static_cast<double>(orientation_array[0]);
            target_area.target_pose.orientation.y = static_cast<double>(orientation_array[1]);
            target_area.target_pose.orientation.z = static_cast<double>(orientation_array[2]);
            target_area.target_pose.orientation.w = static_cast<double>(orientation_array[3]);
            ROS_INFO("pose success: [%.2f, %.2f, %.2f, %.2f]", 
                     target_area.target_pose.orientation.x, 
                     target_area.target_pose.orientation.y, 
                     target_area.target_pose.orientation.z, 
                     target_area.target_pose.orientation.w);
        } else {
            ROS_ERROR("wrong pose");
            success = false;
        }
    } else {
        ROS_ERROR("cannot get pose");
        success = false;
    }
    
    // 设置区域状态
    target_area.active = false;
    
    if (success) {
        ROS_INFO("=== all params load successful ===");
    } else {
        ROS_ERROR("=== failed ===");
        // 设置默认值
            target_area.center.x = 0;
    target_area.center.y =-1;
    target_area.center.z = 0.0;
    target_area.radius = 0.5;
    target_area.adjust_yaw_tolerance = 1.57;
    target_area.target_pose.position.x = 0;
    target_area.target_pose.position.y = -1;
    target_area.target_pose.position.z = 0.0;
    target_area.target_pose.orientation.x = 0.0;
    target_area.target_pose.orientation.y = 0.0;
    target_area.target_pose.orientation.z = 0;
    target_area.target_pose.orientation.w = 1;
    target_area.active = false;
    
    ROS_INFO("default param");
    }
    
}


void AreaNavigator::initializeROSComponents()
{
    // 创建move_base动作客户端
    move_base_client_ = std::make_unique<MoveBaseClient>("move_base", true);
    
    // 等待move_base服务器
    ROS_INFO("waiting for move_base server...");
    move_base_client_->waitForServer();
    ROS_INFO("move_base server has connected");
    
    // 订阅全局导航目标
    goal_sub_ = nh_.subscribe<move_base_msgs::MoveBaseActionGoal>(
        "move_base/goal", 10, &AreaNavigator::goalPoseCallback, this);
    
    // 订阅机器人位姿
    pose_sub_ = nh_.subscribe<geometry_msgs::PoseStamped>(
        "Navservice", 10, &AreaNavigator::robotPoseCallback, this);
    
    // 创建可视化标记发布器
    marker_pub_ = nh_.advertise<visualization_msgs::Marker>("area_markers", 10);
    ROS_INFO("Marker publisher created on topic: %s", marker_pub_.getTopic().c_str());

    // 创建重置服务
    reset_service_ = nh_.advertiseService(
        "reset_area_navigation", &AreaNavigator::resetServiceCallback, this);
    
    // 创建监控定时器
    monitor_timer_ = nh_.createTimer(
        ros::Duration(0.1),  // 10Hz
        &AreaNavigator::monitorNavigation, this);
}

void AreaNavigator::goalPoseCallback(const move_base_msgs::MoveBaseActionGoal::ConstPtr& msg)
{
    if (is_in_adjustment_) {
        ROS_INFO("Adjusting pose, ignoring new target");
        return;
    }
    else{
        ROS_INFO("Not agjust,navigate");
    }
    
    current_goal_ = *msg;
    has_active_goal_ = true;
    
    // ROS_INFO("Received global navigation target: (%.2f, %.2f)", 
    //          msg->goal.target_pose.pose.position.x, msg->goal.target_pose.pose.position.y);
    
    // 直接发送导航目标
    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose = current_goal_.goal.target_pose;
    
    move_base_client_->sendGoal(goal,
        boost::bind(&AreaNavigator::goalDoneCallback, this, _1, _2),
        boost::bind(&AreaNavigator::goalActiveCallback, this),
        boost::bind(&AreaNavigator::goalFeedbackCallback, this, _1));

}

void AreaNavigator::robotPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header = msg->header;
    pose_stamped.pose = msg->pose;
    current_pose_ = pose_stamped;
    ROS_INFO("get pose:%.2f ,%.2f",
    msg->pose.position.x,msg->pose.position.y);
}

void AreaNavigator::monitorNavigation(const ros::TimerEvent& event)
{
    if (!has_active_goal_ || is_in_adjustment_) {
        return;
    }
    
    // 检查是否进入目标区域
    if (isInTargetArea(current_pose_, target_area)) {
        ROS_INFO("Enter the target area and start pose adjustment");
        adjustPoseInArea(target_area);
    }
    else{
            ROS_INFO("not in target area");
        }
}
bool AreaNavigator::isInTargetArea(const geometry_msgs::PoseStamped& pose, TargetArea& area)
{
        // if (target_area.active) continue;  // 跳过已激活的区域
        
        double dx = pose.pose.position.x - target_area.center.x;
        double dy = pose.pose.position.y - target_area.center.y;
        double distance = std::sqrt(dx*dx + dy*dy);
        
        double current_yaw = tf::getYaw(pose.pose.orientation);
        double target_yaw = tf::getYaw(target_area.target_pose.orientation);
        double yaw_error = std::abs(current_yaw - target_yaw);
        
        if (distance <= target_area.radius && yaw_error <= target_area.adjust_yaw_tolerance) {
            area = target_area;
            target_area.active = true;
            return true;
        }
    else{
                ROS_INFO("distance is %.2f" ,
                distance);
            }
    return false;
}

void AreaNavigator::adjustPoseInArea(const TargetArea& area)
{
    is_in_adjustment_ = true;
    
    // 取消当前导航目标
    move_base_client_->cancelAllGoals();
    
    // 创建位姿调整目标
    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose = area.target_pose;
    
    ROS_INFO("Adjust the pose to: (%.2f, %.2f, %.2f)", 
             area.target_pose.position.x, area.target_pose.position.y, 
             tf::getYaw(area.target_pose.orientation));
    
    // 发送位姿调整目标
    move_base_client_->sendGoal(goal,
        boost::bind(&AreaNavigator::goalDoneCallback, this, _1, _2),
        boost::bind(&AreaNavigator::goalActiveCallback, this),
        boost::bind(&AreaNavigator::goalFeedbackCallback, this, _1));
}

void AreaNavigator::resumeNavigation()
{
    if (!has_active_goal_) {
        ROS_WARN("There is no active global target available for restoration");
        return;
    }
    
    ROS_INFO("Restore global navigation target");
    
    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose = current_goal_.goal.target_pose;
    
    move_base_client_->sendGoal(goal,
        boost::bind(&AreaNavigator::goalDoneCallback, this, _1, _2),
        boost::bind(&AreaNavigator::goalActiveCallback, this),
        boost::bind(&AreaNavigator::goalFeedbackCallback, this, _1));
}

void AreaNavigator::goalActiveCallback()
{
    ROS_INFO("Navigation target activated");
}

void AreaNavigator::goalFeedbackCallback(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback)
{
    if (is_in_adjustment_) {
        // 计算当前位置与目标位置的距离
        double dx = feedback->base_position.pose.position.x - current_goal_.goal.target_pose.pose.position.x;
        double dy = feedback->base_position.pose.position.y - current_goal_.goal.target_pose.pose.position.y;
        double distance = std::sqrt(dx*dx + dy*dy);
        ROS_DEBUG("Pose adjustment progress: %.2fmeter", distance);
    }
}

void AreaNavigator::goalDoneCallback(const actionlib::SimpleClientGoalState& state,
                                    const move_base_msgs::MoveBaseResultConstPtr& result)
{
    ROS_INFO("Navigation task completed %s", state.toString().c_str());
    
    if (is_in_adjustment_) {
        is_in_adjustment_ = false;
        ROS_INFO("Pose adjustment completed, resuming global navigation");
        resumeNavigation();
    }
}

bool AreaNavigator::resetServiceCallback(std_srvs::Trigger::Request& req,
                                       std_srvs::Trigger::Response& res)
{
    // 重置所有区域状态

        target_area.active = false;
    
    
    is_in_adjustment_ = false;
    has_active_goal_ = false;
    
    // 取消所有目标
    move_base_client_->cancelAllGoals();
    
    res.success = true;
    res.message = "The regional navigation has been reset";
    ROS_INFO("The regional navigation status has been reset");
    return true;
}
void AreaNavigator::publishVisualizationMarkers()
{
    // ROS_INFO("=== publishVisualizationMarkers CALLED ===");
    
    // 原有的函数内容保持不变，只是添加了参数
    static int publish_count = 0;
    publish_count++;
    // ROS_INFO("Publish count: %d", publish_count);
    
    // 检查是否有订阅者
    int num_subscribers = marker_pub_.getNumSubscribers();
    if (num_subscribers == 0) {
        if (publish_count % 10 == 0) {  // 每10次发布输出一次日志
            ROS_INFO("No subscribers for visualization markers");
        }
        return;
    }
    
    // 区域标记（圆柱体）
    visualization_msgs::Marker area_marker;
    area_marker.header.frame_id = "map";
    area_marker.header.stamp = ros::Time::now();
    area_marker.ns = "target_area";
    area_marker.id = 0;
    area_marker.type = visualization_msgs::Marker::CYLINDER;
    area_marker.action = visualization_msgs::Marker::ADD;
    
    area_marker.pose.position = target_area.center;
    area_marker.pose.orientation.w = 1.0;
    area_marker.scale.x = target_area.radius * 2;
    area_marker.scale.y = target_area.radius * 2;
    area_marker.scale.z = 0.1;  // 高度
    
    // 根据区域状态改变颜色
    if (target_area.active) {
        area_marker.color.r = 1.0;  // 激活时红色
        area_marker.color.g = 0.0;
        area_marker.color.b = 0.0;
    } else {
        area_marker.color.r = 0.0;  // 未激活时蓝色
        area_marker.color.g = 0.5;
        area_marker.color.b = 1.0;
    }
    area_marker.color.a = 0.3;  // 透明度
    
    area_marker.lifetime = ros::Duration(0.2);  // 短生存时间，确保实时更新
    
    // 目标位姿标记（箭头）
    visualization_msgs::Marker target_marker;
    target_marker.header.frame_id = "map";
    target_marker.header.stamp = ros::Time::now();
    target_marker.ns = "target_pose";
    target_marker.id = 1;
    target_marker.type = visualization_msgs::Marker::ARROW;
    target_marker.action = visualization_msgs::Marker::ADD;
    
    target_marker.pose = target_area.target_pose;
    target_marker.scale.x = 0.5;  // 箭头长度
    target_marker.scale.y = 0.1;  // 箭头宽度
    target_marker.scale.z = 0.1;  // 箭头高度
    
    target_marker.color.r = 1.0;  // 红色
    target_marker.color.g = 0.0;
    target_marker.color.b = 0.0;
    target_marker.color.a = 0.3;  // 不透明
    
    target_marker.lifetime = ros::Duration(0.2);

    // 发布标记
    marker_pub_.publish(area_marker);
    marker_pub_.publish(target_marker);
    // ROS_INFO("publish success");
    
    // if (publish_count % 10 == 0) {  // 每10次发布输出一次日志
    //     ROS_INFO("Published visualization markers to %d subscribers", num_subscribers);
        // ROS_INFO("Area center: (%.2f, %.2f, %.2f)", 
        //          target_area.center.x, target_area.center.y, target_area.center.z);
        // ROS_INFO("Target pose: (%.2f, %.2f, %.2f)", 
        //          target_area.target_pose.position.x, 
        //          target_area.target_pose.position.y, 
        //          target_area.target_pose.position.z);
    // }
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "area_navigator");
    AreaNavigator navigator;
    ros::spin();
    return 0;
}